function [x, P] = TungDCM_KF2(x_1, P_1, z)

% x_1: (6x1) previous estimate value [C21;C22;C23;gx;gy;gz]
% P_1: (6x6) previous error covariance
% z  : (6x1) observer value [mx;my;mz;gx;gy;gz]
% x  : (6x1) estimation value at this time
% P  : (6x6) error covariance

%% Init Kalman
T = 0.01;
q = 1;
rm0 = 0.01;
rg0 = 1;
R = [rm0*eye(3,3), zeros(3,3);
      zeros(3,3), rg0*eye(3,3)];
H = eye(6);  

%% Discrete model
C21 = x_1(1);
C22 = x_1(2);
C23 = x_1(3);
C2 = [0, -C23, C22; C23, 0, -C21;-C22, C21, 0];
Phik = [ eye(3) , C2 * T; zeros(3) , eye(3)]; % Phik = I + Phi(kT) * T
Qk = q*[T^3 * C2 *C2', T^2 * C2; T^2 * C2', T * eye(3)];

%% Time update
% State
x_ = Phik * x_1;   
% Error covariance
P_ = Phik * P_1 * Phik' + Qk;    

%% Measurement update
% Compute Kalman gain
Kk = P_ * H' * inv(H * P_*H' + R);
% Update estimate with measurement z
x = x_ + Kk * (z - H * x_);
% Normalization DCM(1:3)
x(1:3) = x(1:3)/norm(x(1:3)); 
% Update the error covariance
P = (eye(6) - Kk * H) * P_; 

end

